#include "CylinderBody.h"
#include <cmath>
#include <qmath.h>
#include <QVector3D>

CylinderBody::CylinderBody(Point position, float radius, float thickness) {
	this->setId(rand());
	this->position->from(position);
	this->radius = radius;
	this->thickness = thickness;
}

void CylinderBody::calcAabb() {

	QQuaternion quaternion = this->getRotation();
	Point position = *this->getPosition();
	if (nullptr != this->upPosition) {
		position += *this->upPosition;
	}

	if (nullptr != this->upRotation) {
		quaternion = *this->upRotation * quaternion;

	}
	Aabb* aabb = this->getAabb();
	aabb->setPosition(position);
	QVector3D points[32];

	// Calculate the points for the cylinder
	float angleStep = 2 * M_PI / 16; // 16 segments for the cylinder
	for (int i = 0; i < 16; ++i) {
		float angle = i * angleStep;
		float x = radius * cos(angle);
		float y = radius * sin(angle);
		/*points[i] = quaternion.rotatedVector(QVector3D(x, y, thickness / 2));
		points[i + 16] = quaternion.rotatedVector(QVector3D(x, y, -thickness / 2));*/

		points[i] = QVector3D(x, y, thickness / 2);
		// Bottom circle points
		points[i + 16] = QVector3D(x, y, -thickness / 2);
	}

	float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
	float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;
	for (int i = 0; i < 32; ++i) {
		minX = std::min(minX, points[i].x());
		minY = std::min(minY, points[i].y());
		minZ = std::min(minZ, points[i].z());
		maxX = std::max(maxX, points[i].x());
		maxY = std::max(maxY, points[i].y());
		maxZ = std::max(maxZ, points[i].z());
	}
	aabb->setMin(Point(minX + position.x(), minY + position.y(), minZ + position.z()));
	aabb->setMax(Point(maxX + position.x(), maxY + position.y(), maxZ + position.z()));
	aabb->setPointSize(32);
	Point* aabbPoints = new Point[aabb->getPointSize()];
	aabbPoints[0] = Point(points[0].x() + position.x(), points[0].y() + position.y(), points[0].z() + position.z());
	aabbPoints[1] = Point(points[1].x() + position.x(), points[1].y() + position.y(), points[1].z() + position.z());
	aabbPoints[2] = Point(points[2].x() + position.x(), points[2].y() + position.y(), points[2].z() + position.z());
	aabbPoints[3] = Point(points[3].x() + position.x(), points[3].y() + position.y(), points[3].z() + position.z());
	aabbPoints[4] = Point(points[4].x() + position.x(), points[4].y() + position.y(), points[4].z() + position.z());
	aabbPoints[5] = Point(points[5].x() + position.x(), points[5].y() + position.y(), points[5].z() + position.z());
	aabbPoints[6] = Point(points[6].x() + position.x(), points[6].y() + position.y(), points[6].z() + position.z());
	aabbPoints[7] = Point(points[7].x() + position.x(), points[7].y() + position.y(), points[7].z() + position.z());
	aabbPoints[8] = Point(points[8].x() + position.x(), points[8].y() + position.y(), points[8].z() + position.z());
	aabbPoints[9] = Point(points[9].x() + position.x(), points[9].y() + position.y(), points[9].z() + position.z());
	aabbPoints[10] = Point(points[10].x() + position.x(), points[10].y() + position.y(), points[10].z() + position.z());
	aabbPoints[11] = Point(points[11].x() + position.x(), points[11].y() + position.y(), points[11].z() + position.z());
	aabbPoints[12] = Point(points[12].x() + position.x(), points[12].y() + position.y(), points[12].z() + position.z());
	aabbPoints[13] = Point(points[13].x() + position.x(), points[13].y() + position.y(), points[13].z() + position.z());
	aabbPoints[14] = Point(points[14].x() + position.x(), points[14].y() + position.y(), points[14].z() + position.z());
	aabbPoints[15] = Point(points[15].x() + position.x(), points[15].y() + position.y(), points[15].z() + position.z());
	aabbPoints[16] = Point(points[16].x() + position.x(), points[16].y() + position.y(), points[16].z() + position.z());
	aabbPoints[17] = Point(points[17].x() + position.x(), points[17].y() + position.y(), points[17].z() + position.z());
	aabbPoints[18] = Point(points[18].x() + position.x(), points[18].y() + position.y(), points[18].z() + position.z());
	aabbPoints[19] = Point(points[19].x() + position.x(), points[19].y() + position.y(), points[19].z() + position.z());
	aabbPoints[20] = Point(points[20].x() + position.x(), points[20].y() + position.y(), points[20].z() + position.z());
	aabbPoints[21] = Point(points[21].x() + position.x(), points[21].y() + position.y(), points[21].z() + position.z());
	aabbPoints[22] = Point(points[22].x() + position.x(), points[22].y() + position.y(), points[22].z() + position.z());
	aabbPoints[23] = Point(points[23].x() + position.x(), points[23].y() + position.y(), points[23].z() + position.z());
	aabbPoints[24] = Point(points[24].x() + position.x(), points[24].y() + position.y(), points[24].z() + position.z());
	aabbPoints[25] = Point(points[25].x() + position.x(), points[25].y() + position.y(), points[25].z() + position.z());
	aabbPoints[26] = Point(points[26].x() + position.x(), points[26].y() + position.y(), points[26].z() + position.z());
	aabbPoints[27] = Point(points[27].x() + position.x(), points[27].y() + position.y(), points[27].z() + position.z());
	aabbPoints[28] = Point(points[28].x() + position.x(), points[28].y() + position.y(), points[28].z() + position.z());
	aabbPoints[29] = Point(points[29].x() + position.x(), points[29].y() + position.y(), points[29].z() + position.z());
	aabbPoints[30] = Point(points[30].x() + position.x(), points[30].y() + position.y(), points[30].z() + position.z());
	aabbPoints[31] = Point(points[31].x() + position.x(), points[31].y() + position.y(), points[31].z() + position.z());

	aabb->setPoints(aabbPoints);

	int SEGMENTS = 16;


	aabb->setIndexSize((SEGMENTS - 2) * 6 + (SEGMENTS) * 6);
	int* indexes = new int[aabb->getIndexSize()];
	//indies
	int index = 0;
	for (int i = 0; i < SEGMENTS - 2; i++) {
		indexes[index++] = 0; // top circle
		indexes[index++] = i + 1; // top circle
		indexes[index++] = i + 2 == SEGMENTS ? 0 : i + 2; // top circle

		indexes[index++] = SEGMENTS + i + 2 == 2 * SEGMENTS ? SEGMENTS : SEGMENTS + i + 2; // bottom circle
		indexes[index++] = SEGMENTS + i + 1; // bottom circle
		indexes[index++] = SEGMENTS; // bottom circle
	}

	for (int i = 0; i < SEGMENTS; i++) {
		indexes[index++] = i; // top circle
		indexes[index++] = SEGMENTS + i; // bottom circle
		indexes[index++] = SEGMENTS + i + 1 == 2 * SEGMENTS ? SEGMENTS : SEGMENTS + i + 1; // top circle

		indexes[index++] = SEGMENTS + i + 1 == 2 * SEGMENTS ? SEGMENTS : SEGMENTS + i + 1; // bottom circle
		indexes[index++] = i + 1 == SEGMENTS ? 0 : i + 1;
		indexes[index++] = i;
	}
	aabb->setIndies(indexes);

}